using BepuUtilities;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuPhysics
{
    // TODO：有点奇怪,它与BepuUtility并存。RigidTransform。最初的推理是刚性姿势可能最终具有非FP32表示。
    // 我们还没有利用这一点,所以现在它几乎是一个纯粹的复制品。
    // 当/如果我们利用较大尺寸的优势,我们将不得不仔细分析RigidPose的每一个用例,看看我们是否需要更高的精度。
    /// <summary>
    /// 表示刚性变换。
    /// </summary>
    public struct RigidPose
    {
        public Vector3 Position;
        // 请注意,我们存储的是四元数而不是矩阵3x3。虽然这在执行矢量变换或提取基矢量时通常需要一些开销,
        // 需要直接与此表示交互的系统通常内存非常有限。花费额外的ALU时间来转换为基数实际上可能会更快
        // 而不是加载表示完整的3x3旋转矩阵所需的额外5个元素。此外,随着时间的推移保持旋转正常化也会稍微容易一些。
        // 可能会有一种观点认为矩阵变量也应该存储在一些带宽不受限制的阶段中,但是在有理由担心之前,不要担心这一点。
        public Quaternion Orientation;

        public static RigidPose Identity { get; } = new RigidPose(new Vector3());

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public RigidPose(in Vector3 position, in Quaternion orientation)
        {
            Position = position;
            Orientation = orientation;
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public RigidPose(in Vector3 position)
        {
            Position = position;
            Orientation = Quaternion.Identity;
        }

        /// <summary>
        /// 按刚性姿势变换向量：v*pose.Orientation+pose.Position。
        /// </summary>
        /// <param name="v">要转换的向量。</param>
        /// <param name="pose">变换向量的姿势。</param>
        /// <param name="result">转换后的向量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Transform(in Vector3 v, in RigidPose pose, out Vector3 result)
        {
            QuaternionEx.TransformWithoutOverlap(v, pose.Orientation, out var rotated);
            result = rotated + pose.Position;
        }
        /// <summary>
        /// 通过刚性姿势的倒数变换向量：(v-pose.Position)*pose.Orientation^-1。
        /// </summary>
        /// <param name="v">要转换的向量。</param>
        /// <param name="pose">用于反转和变换矢量的姿势。</param>
        /// <param name="result">转换后的向量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void TransformByInverse(in Vector3 v, in RigidPose pose, out Vector3 result)
        {
            var translated = v - pose.Position;
            QuaternionEx.Conjugate(pose.Orientation, out var conjugate);
            QuaternionEx.TransformWithoutOverlap(translated, conjugate, out result);
        }
        /// <summary>
        /// 反转姿势的刚性变换。
        /// </summary>
        /// <param name="pose">要反转的姿势。</param>
        /// <param name="Inverse">姿势的反转。</param>
        public static void Invert(in RigidPose pose, out RigidPose inverse)
        {
            QuaternionEx.Conjugate(pose.Orientation, out inverse.Orientation);
            QuaternionEx.Transform(-pose.Position, inverse.Orientation, out inverse.Position);
        }

        /// <summary>
        /// 将一个刚性变换与另一个刚性变换连接起来。所得到的变换等同于先执行变换a,然后执行变换b。
        /// </summary>
        /// <param name="a">要连接的第一个转换。</param>
        /// <param name="b">要连接的第二个转换。</param>
        /// <param name="result">串联的结果。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void MultiplyWithoutOverlap(in RigidPose a, in RigidPose b, out RigidPose result)
        {
            QuaternionEx.ConcatenateWithoutOverlap(a.Orientation, b.Orientation, out result.Orientation);
            QuaternionEx.Transform(a.Position, b.Orientation, out var rotatedTranslationA);
            result.Position = rotatedTranslationA + b.Position;
        }

        public override string ToString()
        {
            return $"位置:{Position} 旋转:{Orientation}";
        }
    }

    public struct BodyVelocity
    {
        public Vector3 Linear;
        public Vector3 Angular;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public BodyVelocity(in Vector3 linear)
        {
            Linear = linear;
            Angular = default;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public BodyVelocity(in Vector3 linear, in Vector3 angular)
        {
            Linear = linear;
            Angular = angular;
        }
    }

    public struct BodyInertia
    {
        public Symmetric3x3 InverseInertiaTensor;
        public float InverseMass;
    }

    public struct RigidPoses
    {
        public Vector3Wide Position;
        public QuaternionWide Orientation;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Broadcast(in RigidPose pose, out RigidPoses poses)
        {
            Vector3Wide.Broadcast(pose.Position, out poses.Position);
            QuaternionWide.Broadcast(pose.Orientation, out poses.Orientation);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void WriteFirst(in RigidPose pose, ref RigidPoses poses)
        {
            Vector3Wide.WriteFirst(pose.Position, ref poses.Position);
            QuaternionWide.WriteFirst(pose.Orientation, ref poses.Orientation);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ReadFirst(in RigidPoses poses, out RigidPose pose)
        {
            Vector3Wide.ReadFirst(poses.Position, out pose.Position);
            QuaternionWide.ReadFirst(poses.Orientation, out pose.Orientation);
        }
    }

    public struct BodyVelocities
    {
        public Vector3Wide Linear;
        public Vector3Wide Angular;
    }

    public struct BodyInertias
    {
        public Symmetric3x3Wide InverseInertiaTensor;
        // 请注意,反质量包含在BodyInertias束中。InverseMasss是旋转不变的,所以不需要更新.
        // 但是它被包括在旋转惯性张量旁边,因为拆分它将需要约束前置步骤在以下情况下遭受另一次高速缓存未命中
        // 孤立地聚集反质量。(从解算器的角度来看,惯性/质量聚集是不连贯的。)
        public Vector<float> InverseMass;
    }

    public struct BodyActivity
    {
        /// <summary>
        /// 允许身体进入睡眠状态的平方速度阈值。这与dot(linearVelocity,linearVelocity)+dot(angularVelocity,angularVelocity)进行比较。
        /// 将其设置为负值可确保身体在没有用户操作的情况下无法进入睡眠状态。
        /// </summary>
        public float SleepThreshold;
        /// <summary>
        /// 在身体成为睡眠候选项之前身体必须低于睡眠阈值的时间步数。
        /// 请注意,不能保证身体在达到此最低要求后立即入睡。
        /// </summary>
        public byte MinimumTimestepsUnderThreshold;

        // 请注意,超过该点的所有值都是运行时设置的。用户实际上永远不需要修改它们。
        // 默认情况下,我们不限制写访问,而是选择让高级用户随意操作。
        // TODO：如果人们滥用这些,我们应该在个案的基础上将它们内部化。

        /// <summary>
        /// 如果身体是清醒的,这是身体的速度低于睡眠阈值的时间步数。
        /// </summary>
        public byte TimestepsUnderThresholdCount;
        // 请注意,尽管遍历只需要SleepCandidate状态,此标志仍与其他休眠数据一起保存。
        // 这主要是为了简单起见,但也要注意,此字段的主要访问器实际上是睡眠候选计算。遍历不会访问每个
        // 身体的每一帧,但睡眠候选分析是这样做的。
        // 此标志存在的原因只是为了防止遍历意识到候选人资格管理背后的逻辑。
        // 存储它不需要任何额外费用;它适合8字节的布局。
        /// <summary>
        /// 如果这个身体适合睡觉,那就是真的。如果它通过约束连接到的所有身体也都是候选的,那么这个身体可能会进入睡眠状态。
        /// </summary>
        public bool SleepCandidate;
    }
}
